import os
import time
import rclpy
import cv2
from rclpy.node import Node
import face_recognition
from ament_index_python.packages import get_package_share_directory
from chapt4_interfaces.srv import FaceDetector # 导入服务接口
from cv_bridge import CvBridge
from rcl_interfaces.msg import SetParametersResult

class FaceDetectNode(Node):
    def __init__(self):
        super().__init__('face_detect_node')
        self.service = self.create_service(FaceDetector, 'face_detect', self.face_detect_callback)
        self.bridge = CvBridge()  #  创建cv_bridge对象
        '''
        声明默认参数 number_of_times_to_upsample，默认值为1。
        声明默认参数 model，默认值为hog。
        获取参数 number_of_times_to_upsample 的值并赋给类的属性。
        获取参数 model 的值并赋给类的属性。
        '''
        self.declare_parameter('number_of_times_to_upsample', 1)
        self.declare_parameter('model', 'hog')
        self.number_of_times_to_upsample = self.get_parameter('number_of_times_to_upsample').value
        self.module = self.get_parameter('model').value
        self.get_logger().info(f'人脸识别服务启动成功。。。。。。。。。。。。。。。')
        self.add_on_set_parameters_callback(self.parameters_callback) #  回调函数
    def parameters_callback(self, params):
        for param in params:
            self.get_logger().info(f'{param.name} -> {param.value}')
            if param.name == 'number_of_times_to_upsample':
                self.number_of_times_to_upsample = param.value
            if param.name == 'model':
                self.module = param.value
        return SetParametersResult(successful=True)
    def face_detect_callback(self, request, response):
        if request.image.data:
            cv_image = self.bridge.imgmsg_to_cv2(request.image) #  将ROS2图像消息转换为OpenCV格式

        else:
            cv_image = cv2.imread(os.path.join(get_package_share_directory('demo_python_service'), 'resource/face.jpg'))
            self.get_logger().info(f'未收到图像消息，使用默认图片。。。。。。。。。.')
        start_time = time.time()
        self.get_logger().info(f'开始识别。。。。。。。。。。。。。。。')
        face_locations = face_recognition.face_locations(cv_image,number_of_times_to_upsample=self.number_of_times_to_upsample,model=self.module)
        """
        sensor_msgs/Image image # 人脸的图像
        ---
        int16 number # 人脸的数量
        float32 use_time # 人脸检测的时间
        int32[] top # 人脸的top坐标
        int32[] left 
        int32[] right 
        int32[] bottom 

        """
        response.use_time = time.time() - start_time
        response.number = len(face_locations)

        for top ,right,bottom,left in face_locations:
                response.top.append(top) 
                response.right.append(right)
                response.bottom.append(bottom)
                response.left.append(left)
        self.get_logger().info(f'识别完成，人脸数量为：{response.number}，耗时：{response.use_time}s。。。。。。。。。。。。。。。')
        return response  #  返回响应


def main():
    rclpy.init()
    node = FaceDetectNode()
    rclpy.spin(node)
    rclpy.shutdown()